/**
 *Synchronous alignment example
 *
 * In this example, the mirror status of the depth and the color may be inconsistent because the depth or color sensor does not support mirroring.
 * As a result, the images displayed by the depth  and the color are reversed. If this happens, just set the mirror interface to keep the two mirror states
 * consistent.
 * In addition, the resolution obtained by some devices may not support the D2C function, so the resolutions that support D2C, please refer to the
 * product manual.
 * For example: the D2C resolution supported by DaBai DCW is 640x360, but the actual resolution obtained in this example may be 640x480, at this time the user
 * must refer to the product manual to select 640x360 resolution.
 */

#include <libobsensor/ObSensor.hpp>
#include "libobsensor/hpp/Pipeline.hpp"
#include "libobsensor/hpp/Error.hpp"
#include <opencv2/opencv.hpp>
#include <mutex>
#include <thread>

static bool  sync  = false;
static float alpha = 0.5;

int get_aligned_image(const char* serialNumber, cv::Mat& color_image, cv::Mat& depth_image)
{
    ob::Context::setLoggerSeverity(OB_LOG_SEVERITY_WARN);
    ob::Pipeline pipe;

    // Configure which streams to enable or disable for the Pipeline by creating a Config
    std::shared_ptr<ob::Config> config = std::make_shared<ob::Config>();

    // Get all stream profiles of the color camera, including stream resolution, frame rate, and frame format
    auto colorProfiles = pipe.getStreamProfileList(OB_SENSOR_COLOR);
    // auto colorProfile = colorProfiles->getProfile(OB_PROFILE_DEFAULT);
    auto colorProfile = colorProfiles->getVideoStreamProfile(1280, 720, OB_FORMAT_RGB, 30);
    config->enableStream(colorProfile);

    // Get all stream profiles of the depth camera, including stream resolution, frame rate, and frame format
    auto depthProfiles = pipe.getStreamProfileList(OB_SENSOR_DEPTH);
    // auto depthProfile = depthProfiles->getProfile(OB_PROFILE_DEFAULT);
    auto depthProfile = depthProfiles->getVideoStreamProfile(1280, 720, OB_FORMAT_Y16, 30);
    config->enableStream(depthProfile);

    /* Config depth align to color or color align to depth.
    OBStreamType align_to_stream = OB_STREAM_DEPTH; */
    OBStreamType align_to_stream = OB_STREAM_COLOR;
    ob::Align    align(align_to_stream);

    // Start the pipeline with config
    try {
        pipe.start(config);
    }
    catch(ob::Error &e) {
        std::cerr << "function:" << e.getName() << "\nargs:" << e.getArgs() << "\nmessage:" << e.getMessage() << "\ntype:" << e.getExceptionType() << std::endl;
        return 1;
    }

    while(true) {

        auto frameSet = pipe.waitForFrames(100);
        if(frameSet == nullptr) {
            continue;
        }

        auto colorFrame = frameSet->colorFrame();
        auto depthFrame = frameSet->depthFrame();

        if(colorFrame != nullptr && depthFrame != nullptr) {
            auto newFrame    = align.process(frameSet);
            auto newFrameSet = newFrame->as<ob::FrameSet>();
            colorFrame       = newFrameSet->colorFrame();
            depthFrame       = newFrameSet->depthFrame();

            // transform frame
            auto videoFrame = colorFrame->as<ob::VideoFrame>();
            cv::Mat color(cv::Size(1280, 720), CV_8UC3, videoFrame->data(), cv::Mat::AUTO_STEP);
            color_image = color.clone();

            videoFrame = depthFrame->as<ob::VideoFrame>();
            cv::Mat depth(cv::Size(1280, 720), CV_16UC1, videoFrame->data(), cv::Mat::AUTO_STEP);
            depth_image = depth.clone();

            break;
        }
    }

    // Stop the Pipeline, no frame data will be generated
    pipe.stop();
    return 0;
}


int main(int argc, char **argv) try {

    cv::Mat color_image;
    cv::Mat depth_image;

    int result = get_aligned_image("1231212", color_image, depth_image);
    if (result)
    {
        std::cerr << "something wrong" << std::endl;
        return 0;
    }
    
    std::cout << "got aligned depth to color" << std::endl;

    cv::imshow("color_image", color_image);
	cv::waitKey(0);
    cv::imshow("depth_image", depth_image);
	cv::waitKey(0);

    return 0;
}
catch(ob::Error &e) {
    std::cerr << "function:" << e.getName() << "\nargs:" << e.getArgs() << "\nmessage:" << e.getMessage() << "\ntype:" << e.getExceptionType() << std::endl;
    exit(EXIT_FAILURE);
}
